#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>
#include <cv.h>
#include <highgui.h>
#include <opencv2/aruco.hpp>

#include <stdio.h>
#include <iostream>

#include <ros/ros.h>
#include <ros/package.h>

#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>
#include <image_transport/image_transport.h>
#include <std_msgs/Header.h>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <yaml-cpp/yaml.h>
#include <yaml-cpp/node/parse.h>

// #define Debug True


// agv位点位姿结构体
struct AgvPoint
{
    std::string name;
    geometry_msgs::Point position;
    geometry_msgs::Quaternion orientation;
};


// yaml库读取以及存储代码
namespace YAML
{
    template <>
    struct convert<geometry_msgs::Point>
    {
        static Node encode(const geometry_msgs::Point &gp)
        {
            Node node;
            node.push_back(gp.x);
            node.push_back(gp.y);
            node.push_back(gp.z);
            return node;
        }

        static bool decode(const Node &node, geometry_msgs::Point &gp)
        {
            gp.x = node["x"].as<double>();
            gp.y = node["y"].as<double>();
            gp.z = node["z"].as<double>();
            return true;
        }
    };

    template <>
    struct convert<geometry_msgs::Quaternion>
    {
        static Node encode(const geometry_msgs::Quaternion &gq)
        {
            Node node;
            node.push_back(gq.x);
            node.push_back(gq.y);
            node.push_back(gq.z);
            node.push_back(gq.w);
            return node;
        }

        static bool decode(const Node &node, geometry_msgs::Quaternion &gq)
        {
            gq.x = node["x"].as<double>();
            gq.y = node["y"].as<double>();
            gq.z = node["z"].as<double>();
            gq.w = node["w"].as<double>();
            return true;
        }
    };

    template <>
    struct convert<AgvPoint>
    {
        static Node encode(const AgvPoint &ap)
        {
            Node node;
            node.push_back(ap.name);
            node.push_back(ap.position);
            node.push_back(ap.orientation);
            return node;
        }

        static bool decode(const Node &node, AgvPoint &ap)
        {
            ap.name = node["name"].as<std::string>();
            ap.position = node["position"].as<geometry_msgs::Point>();
            ap.orientation = node["orientation"].as<geometry_msgs::Quaternion>();
            return true;
        }
    };
}

#define FILTER_KERNEL 5


ros::Publisher aruco_pose_pub;
image_transport::Publisher aruco_image_pub;
cv::Mat origin_image, image_copy;

// 相机内参
double fx, fy, cx, cy, k1, k2, k3, p1, p2, k4, k5, k6;
cv::Mat camera_matrix;
cv::Mat dist_coeffs;
cv::Ptr<cv::aruco::Dictionary> dictionary;

// 均值滤波用数组
std::vector<std::vector<double>> origin_position(FILTER_KERNEL, std::vector<double>(3, 0));
std::vector<Eigen::Matrix4d> origin_orientation(FILTER_KERNEL, Eigen::Matrix4d::Identity());
int flag = -1;

std::vector<AgvPoint> agv_positons;


// 获得矩阵的最大特征根
void getMaxEigenVector(Eigen::Matrix4d &M, Eigen::Vector4d &eigen_vector_max)
{
    Eigen::EigenSolver<Eigen::Matrix4d> eigen_solver(M);
    Eigen::MatrixXcd eigen_vector = eigen_solver.eigenvectors();
    Eigen::MatrixXd eigen_value = eigen_solver.eigenvalues().real();
    Eigen::MatrixXd::Index evals_max;
    eigen_value.rowwise().sum().maxCoeff(&evals_max);
    eigen_vector_max = eigen_vector.col(evals_max).real();
}

// 均值滤波四元数，参考https://www.cnblogs.com/21207-iHome/p/6952004.html
void quaternionAverage(std::vector<Eigen::Matrix4d> &origin_orientation, Eigen::Quaterniond &average_quaternion)
{
    Eigen::Matrix4d sum_orientation = Eigen::Matrix4d::Zero();
    for (int i = 0; i < FILTER_KERNEL; ++i)
    {
        for (int m = 0; m < 4; ++m)
        {
            for (int n = 0; n < 4; ++n)
            {
                sum_orientation(m, n) += origin_orientation[i](m, n);
            }
        }
    }
    for (int m = 0; m < 4; ++m)
    {
        for (int n = 0; n < 4; ++n)
        {
            sum_orientation(m, n) /= FILTER_KERNEL;
        }
    }
    Eigen::Vector4d temp = Eigen::Vector4d::Zero();
    getMaxEigenVector(sum_orientation, temp);
    average_quaternion = Eigen::Quaterniond(temp(0), temp(1), temp(2), temp(3));
}

// 四元数矩阵赋值
void assignMatrixWithOtherMatrix(Eigen::Matrix4d &T, Eigen::Matrix4d &other)
{
    for (int i = 0; i < 4; i++)
    {
        for (int j = 0; j < 4; j++)
        {
            T(i, j) = other(i, j);
        }
    }
}

// image回调函数
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
{
    ROS_INFO("imageCallback");
    origin_image = cv_bridge::toCvShare(msg, "bgr8")->image;
    origin_image.copyTo(image_copy);


    std::vector<int> ids;
    std::vector<std::vector<cv::Point2f>> corners;
    cv::aruco::detectMarkers(origin_image, dictionary, corners, ids); //检测靶标

    std::cout << "the num of marker detected is: " << ids.size() << std::endl;

    // agv位点信息
    int agv_point_index = -1;

    // 检测到Aruco标志
    if (ids.size() == 1)
    {
        cv::aruco::drawDetectedMarkers(image_copy, corners, ids); //绘制检测到的靶标的框
        std::vector<cv::Vec3d> rvecs, tvecs;

        // 利用opencv的aruco库计算靶标的旋转和平移
        cv::aruco::estimatePoseSingleMarkers(corners, 0.180, camera_matrix, dist_coeffs, rvecs, tvecs); //求解旋转矩阵rvecs和平移矩阵tvec

        Eigen::Vector3d v;
        v << rvecs[0][0], rvecs[0][1], rvecs[0][2];
        double theta = v.norm();
        v.normalize();
        Eigen::AngleAxisd rotation_vector(theta, v);
        Eigen::Quaterniond q(rotation_vector);
        Eigen::Matrix4d T;
        std::vector<double> temp = {q.w(), q.x(), q.y(), q.z()};
        for (int i = 0; i < 4; ++i)
        {
            for (int j = 0; j < 4; ++j)
            {
                T(i, j) = temp[i] * temp[j];
            }
        }

        Eigen::Matrix3d R = q.toRotationMatrix();
        std::cout << "single aruco id is: " << ids[0] << std::endl;
        std::cout << "single aruco T:" << tvecs[0] << std::endl;
        std::cout << "single aruco the rotation matrix is\n"
                  << R << std::endl;
        std::cout << "-----" << std::endl;
        cv::aruco::drawAxis(image_copy, camera_matrix, dist_coeffs, rvecs[0], tvecs[0], 0.1);

        // 如果是第一帧，则将该帧数据赋给滤波数组中的每一帧
        if (flag == -1)
        {

            for (int i = 0; i < origin_position.size(); ++i)
            {
                // std::cout<<i<<std::endl;
                for (int j = 0; j < 3; ++j)
                {
                    // std::cout<<j<<std::endl;
                    origin_position[i][j] = tvecs[0][j] * 1000;
                }
                assignMatrixWithOtherMatrix(origin_orientation[i], T);
            }
            flag = 1;
#ifdef Debug
            std::cout << "flag" << std::endl;
#endif
        }
        // 否则,将该一帧数据赋给滤波数组中对应帧
        else
        {
            for (int j = 0; j < 3; ++j)
            {
                origin_position[flag][j] = tvecs[0][j] * 1000;
            }
            assignMatrixWithOtherMatrix(origin_orientation[flag], T);
            flag = (flag + 1) % FILTER_KERNEL;
        }
    }
    // 如果检测到Aruco标志不为1
    else
    {
        // 将第一帧标志位设置为-1
        flag = -1;
        // 如果未检测到Aruco标志,则直接读取事先标记好的位姿
        if (ids.empty())
        {
            ROS_ERROR("no aruco detected");
            ros::param::get("/agv/agv_point_index", agv_point_index);
        }
        // 如果检测到多个Aruco标志(一般不会出现)
        else
        {
            ROS_WARN("multi aruco detected");
            cv::aruco::drawDetectedMarkers(image_copy, corners, ids); //绘制检测到的靶标的框
            std::vector<cv::Vec3d> rvecs, tvecs;
            cv::aruco::estimatePoseSingleMarkers(corners, 0.180, camera_matrix, dist_coeffs, rvecs, tvecs); //求解旋转矩阵rvecs和平移矩阵tvecs
            for (int i = 0; i < ids.size(); ++i)
            {
                Eigen::Vector3d v;
                v << rvecs[i][0], rvecs[i][1], rvecs[i][2];
                double theta = v.norm();
                v.normalize();
                Eigen::AngleAxisd rotation_vector(theta, v);
                Eigen::Quaterniond q(rotation_vector);

                Eigen::Matrix3d R = q.toRotationMatrix();
                std::cout << "multi aruco id is: " << ids[i] << std::endl;
                std::cout << "multi aruco T:" << tvecs[i] << std::endl;
                std::cout << "multi aruco the rotation matrix is\n"
                          << R << std::endl;
                cv::aruco::drawAxis(image_copy, camera_matrix, dist_coeffs, rvecs[i], tvecs[i], 0.1 * (i + 1));
                std::cout << "-----" << std::endl;
            }
        }
    }

    geometry_msgs::Pose pose_msg;
    // 判断是否检测到Aruco标志
    if (flag == -1)
    {
        // 如果未检测到Aruco标志且没读取到事先标记好的位姿,则报错
        if (agv_point_index == -1)
        {
            ROS_ERROR("agv_point_index is not set");
            pose_msg.position.z = 0;
            pose_msg.position.x = 0;
            pose_msg.position.y = 0;
            pose_msg.orientation.x = 0;
            pose_msg.orientation.y = 0;
            pose_msg.orientation.z = 0;
            pose_msg.orientation.w = 1;
        }
        // 如果未检测到Aruco标志，但知道Agv运动到几号点，则直接读取对应位姿的信息
        else
        {
            ROS_INFO("select agv pose from config file");
            pose_msg.position.x = agv_positons[agv_point_index].position.x;
            pose_msg.position.y = agv_positons[agv_point_index].position.y;
            pose_msg.position.z = agv_positons[agv_point_index].position.z;
            pose_msg.orientation.x = agv_positons[agv_point_index].orientation.x;
            pose_msg.orientation.y = agv_positons[agv_point_index].orientation.y;
            pose_msg.orientation.z = agv_positons[agv_point_index].orientation.z;
            pose_msg.orientation.w = agv_positons[agv_point_index].orientation.w;
        }
    }
    else
    {
        // 均值滤波位置信息
        std::vector<double> temp(3, 0);
        for (int i = 0; i < 3; ++i)
        {
            for (int j = 0; j < FILTER_KERNEL; ++j)
            {
                temp[i] += origin_position[j][i];
            }
            temp[i] /= FILTER_KERNEL;
        }

        Eigen::Quaterniond q;
#ifdef Debug
        std::cout << "average" << std::endl;
#endif
        // 均值滤波姿态信息
        quaternionAverage(origin_orientation, q);
#ifdef Debug
        std::cout << "average completed" << std::endl;
#endif
        pose_msg.position.x = temp[0];
        pose_msg.position.y = temp[1];
        pose_msg.position.z = temp[2];
        pose_msg.orientation.x = q.x();
        pose_msg.orientation.y = q.y();
        pose_msg.orientation.z = q.z();
        pose_msg.orientation.w = q.w();

        Eigen::Matrix3d R = q.toRotationMatrix();
        std::cout << "the tranlation is: " << temp[0] << "\t" << temp[1] << "\t" << temp[2] << std::endl;
        std::cout << "the rotation matrix is\n"
                  << R << std::endl;
    }
    // 将位姿信息发送出去
    sensor_msgs::ImagePtr image_copy_msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_copy).toImageMsg();
    aruco_image_pub.publish(image_copy_msg);
    aruco_pose_pub.publish(pose_msg);
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "aruco_detect_node");
    ros::NodeHandle nh;


    // 加载agv位点信息，若Aruco码未识别，则使用提前标记号的位点

    // 获得agv位点位姿配置文件的路径
    std::string agv_position_param_name;
    nh.param<std::string>("agv_position_param_name", agv_position_param_name, "agv_position.yaml");
    agv_positons.resize(0);
    std::string dir_package = ros::package::getPath("nuc_communicate");
    std::string dir_param_file = dir_package + "/scripts/config/" + agv_position_param_name;

    // 读取agv位点位姿配置文件
    YAML::Node config;
    try
    {
        config = YAML::LoadFile(dir_param_file);
    }
    catch (YAML::BadFile &e)
    {
        std::cout << "error during reading agv position file: " << dir_param_file << std::endl;
        return -1;
    }
    geometry_msgs::Pose temp;

    if (config["agv_points"])
    {
        agv_positons = config["agv_points"].as<std::vector<AgvPoint>>();
    }

    std::cout << "agv position param parse complete" << std::endl;
    std::cout << "the size of agv point is:" << agv_positons.size() << std::endl;



    cv::namedWindow("aruco_detector", 1);

    // 定义ROS发布者以及订阅者
    image_transport::ImageTransport it(nh);
    aruco_image_pub = it.advertise("/aruco/aruco_image_result", 1);
    aruco_pose_pub = nh.advertise<geometry_msgs::Pose>("/aruco/aruco_pose", 1);
    ros::Subscriber image_sub = nh.subscribe("/k4a/color/color", 1, imageCallback);

    ROS_INFO("aurco pose node init completed");

    // 相机内参参数
    fx = 974.02;
    fy = 973.636;
    cx = 1022.65;
    cy = 773.947;
    k1 = 0.465018;
    k2 = -2.39;
    k3 = 1.33069;
    k4 = 0.3479877710342407;
    k5 = -2.2303285598754883;
    k6 = 1.2691079378128052;
    p1 = 0.000794036;
    p2 = 1.23308e-6;
    camera_matrix = (cv::Mat_<float>(3, 3) << fx, 0.0, cx,
                     0.0, fy, cy,
                     0.0, 0.0, 1.0);
    dist_coeffs = (cv::Mat_<float>(8, 1) << k1, k2, p1, p2, k3, k4, k5, k6);

    // aruco码字典
    dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_7X7_1000);
    ROS_INFO("parameter init completed");

    origin_orientation[0] = Eigen::Matrix4d::Identity();

    ROS_INFO("node spin");

    ros::spin();
    return 0;
}
